#!/usr/bin/env python


import threading
import time

from move_base_msgs.msg import *


_goal_list = []
_gen_goal_thread = None


def get_goal():
    if len(_goal_list)>0:
        return True, _goal_list.pop(0)
    return False, None


def gen_goal():
    global _gen_goal_thread
    _gen_goal_thread = threading.Thread(target=_gen_goal, name="GenGoal")
    _gen_goal_thread.setDaemon(True)
    _gen_goal_thread.start()
    return


def _gen_goal():
    x, y = 0, 0
    for i in range(1, 20):
        x, y = x+i, y+i
        goal = MoveBaseGoal()
        goal.target_pose.pose.position.x, goal.target_pose.pose.position.y = x, y
        _goal_list.append(goal)
        time.sleep(10)
    return


if __name__ == "__main__":
    gen_goal()
    while True:
        flag, goal = get_goal()
        if flag:
            print(goal)
        time.sleep(5)


